/*
 * <nav_recip_common/vector2.h>
 * nav_recip_common
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#ifndef NAV_RECIP_COMMON_VECTOR2_H_
#define NAV_RECIP_COMMON_VECTOR2_H_

#define HAS_SSE_EXTENSIONS

#include <cmath>
#include <ostream>
#include <vector>
#ifdef HAS_SSE_EXTENSIONS
# include <xmmintrin.h>
#else
# include <algorithm>
#endif
#include <ros/assert.h>
#include <ros/macros.h>
#include <nav_recip_common/scalar.h>

namespace nav_recip
{
class Vector2;

/*!
 * \relates Vector2
 *
 * A list of Vector2 objects.
 */
typedef std::vector<Vector2> Vector2List;

/*!
 * \relates Vector2
 * An iterator for a Vector2List.
 */
typedef std::vector<Vector2>::iterator Vector2Iterator;

/*!
 * \relates Vector2
 * A \c const iterator for a Vector2List.
 */
typedef std::vector<Vector2>::const_iterator Vector2ConstIterator;

#ifdef HAS_SSE_EXTENSIONS
// Return an __m128 containing the dot product of the two lowest floating point
// values in two given __m128.
ROS_FORCE_INLINE ROS_HELPER_LOCAL __m128 DotM128(const __m128 m128a, const __m128 m128b);
  
// Store the lowest floating point value from the given __m128.
ROS_FORCE_INLINE ROS_HELPER_LOCAL float StoreM128(const __m128 m128);
#endif

/*!
 * \headerfile <nav_recip/vector2.h>
 *
 * A point or vector in 2D.
 */
class ROS_HELPER_LOCAL Vector2
{
public:
  /*!
   * Create a new Vector2 with zero x-coordinate and y-coordinate.
   */
  ROS_FORCE_INLINE Vector2()
#ifdef HAS_SSE_EXTENSIONS
  {
    m128_ = _mm_setzero_ps();
  }
#else
  : x_(0.0f), y_(0.0f) { }
#endif

  /*!
   * Create a new Vector2 with given x-coordinate and y-coordinate.
   */
  ROS_FORCE_INLINE Vector2(float x, float y) 
#ifdef HAS_SSE_EXTENSIONS
  {
    m128_ = _mm_setr_ps(x, y, 0.0f, 0.0f);
  }
#else
  : x_(x), y_(y) { }
#endif
  
#ifdef HAS_SSE_EXTENSIONS
  // Create a new Vector2 with coordinates equal to the two lower floating point
  // values from the given __m128.
  ROS_FORCE_INLINE Vector2(const __m128 m128) : m128_(m128) { }
#endif

  /*!
   * Copy a Vector2.
   */
  ROS_FORCE_INLINE Vector2(const Vector2& vector) 
#ifdef HAS_SSE_EXTENSIONS
      : m128_(vector.m128_) { }
#else
      : x_(vector.x_), y_(vector.y_) { }
#endif

  /*!
   * Return the x-coordinate of this Vector2.
   */
  ROS_FORCE_INLINE float getX() const
  {
#ifdef HAS_SSE_EXTENSIONS
    return StoreM128(m128_);
#else
    return x_;
#endif
  }

  /*!
   * Return the y-coordinate of this Vector2.
   */
  ROS_FORCE_INLINE float getY() const
  { 
#ifdef HAS_SSE_EXTENSIONS   
    return StoreM128(_mm_shuffle_ps(m128_, m128_, _MM_SHUFFLE(1,1,1,1)));
#else
    return y_;
#endif
  }

  /*!
   * Set the x-coordinate of this Vector2.
   */
  ROS_FORCE_INLINE void setX(float x)
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_setr_ps(x, getY(), 0.0f, 0.0f);
#else
    x_ = x;
#endif
}

  /*!
   * Set the y-coordinate of this Vector2.
   */
  ROS_FORCE_INLINE void setY(float y)
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_setr_ps(getX(), y, 0.0f, 0.0f);
#else
    y_ = y;
#endif
  }
  
  /*!
   * Return this Vector2 with each coordinate equal to the absolute value of its 
   * original value.
   */
  ROS_FORCE_INLINE Vector2& setAbsolute()
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_andnot_ps(_mm_set1_ps(-0.0f), m128_);
#else
    x_ = std::fabs(x_);
    y_ = std::fabs(y_);
#endif
    return *this;
  }
  
  /*!
   * Return a Vector2 with each coordinate equal to the absolute value of the 
   * corresponding coordinate of this Vector2.
   */
  ROS_FORCE_INLINE Vector2 absolute() const
  {
    return Vector2(*this).setAbsolute();
  }

  /*!
   * Return the angle in radians between this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE float angle(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    return std::acos(StoreM128(_mm_div_ps(DotM128(m128_, vector.m128_),  _mm_sqrt_ps(_mm_mul_ps(DotM128(m128_, m128_), 
        DotM128(vector.m128_, vector.m128_))))));
#else
    const float scalar = length2() * vector.length2();

    ROS_ASSERT(scalar > 0.0f);

    return std::acos(dot(vector) / std::sqrt(scalar));
#endif
  }

  /*!
   * Return the cross product of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE float cross(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    __m128 m128 = _mm_mul_ps(m128_, _mm_shuffle_ps(vector.m128_, vector.m128_, _MM_SHUFFLE(3,2,0,1)));
    
    return StoreM128(_mm_sub_ps(_mm_shuffle_ps(m128, m128, _MM_SHUFFLE(0,0,0,0)), _mm_shuffle_ps(m128, m128, _MM_SHUFFLE(1,1,1,1))));
#else
    return x_ * vector.y_ - y_ * vector.x_;
#endif
  }

  /*!
   * Return the Euclidean distance squared between this Vector2 and a given
   * Vector2.
   */
  ROS_FORCE_INLINE float distance2(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    const __m128 m128 = _mm_sub_ps(m128_, vector.m128_);
    
    return StoreM128(DotM128(m128, m128));
#else
    return (*this - vector).length2();
#endif
  }

  /*!
   * Return the Euclidean distance between this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE float distance(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    const __m128 m128 = _mm_sub_ps(m128_, vector.m128_);
    
    return StoreM128(_mm_sqrt_ps(DotM128(m128, m128)));
#else
    return std::sqrt(distance2(vector));
#endif
  }

  /*!
   * Return the dot product of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE float dot(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS    
    return StoreM128(DotM128(m128_, vector.m128_));
#else
    return x_ * vector.x_ + y_ * vector.y_;
#endif
  }

  /*!
   * Return the Euclidean length squared of this Vector2.
   */
  ROS_FORCE_INLINE float length2() const
  {
#ifdef HAS_SSE_EXTENSIONS
    return StoreM128(DotM128(m128_, m128_));
#else
    return dot(*this);
#endif
  }

  /*!
   * Return the Euclidean length of this Vector2.
   */
  ROS_FORCE_INLINE float length() const
  {
#ifdef HAS_SSE_EXTENSIONS
    return StoreM128(_mm_sqrt_ps(DotM128(m128_, m128_)));
#else
    return std::sqrt(length2());
#endif
  }
  
  /*!
   * Return a Vector2 with each coordinate equal to the maximum value of the 
   * corresponding coordinate of this Vector2.
   */
  ROS_FORCE_INLINE Vector2 max(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    return Vector2(_mm_max_ps(m128_, vector.m128_));
#else
    return Vector2(std::max(x_, vector.x_), std::max(y_, vector.y_));
#endif
  }
  
  /*!
   * Return a Vector2 with each coordinate equal to the minimum value of the 
   * corresponding coordinate of this Vector2.
   */
  ROS_FORCE_INLINE Vector2 min(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    return Vector2(_mm_min_ps(m128_, vector.m128_));
#else
    return Vector2(std::min(x_, vector.x_), std::min(y_, vector.y_));
#endif
  }

  /*!
   * Return this Vector2 normalized.
   */
  ROS_FORCE_INLINE Vector2& normalize()
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_mul_ps(m128_, _mm_rsqrt_ps(DotM128(m128_, m128_)));
    
    return *this;
#else
    const float scalar = length();

    ROS_ASSERT(scalar > 0.0f);

    return *this /= scalar;
#endif
  }

  /*!
   * Return a normalized Vector2 with the direction of this Vector2.
   */
  ROS_FORCE_INLINE Vector2 normalized() const
  {
    return Vector2(*this).normalize();
  }

  /*!
   * Return the orientation in radians between this Vector2 and the positive
   * x-axis.
   */
  ROS_FORCE_INLINE float orientation() const
  {
#ifdef HAS_SSE_EXTENSIONS
    return std::atan2(getY(), getX());
#else
    return std::atan2(y_, x_);
#endif
  }
  
  /*!
   * Return a Vector2 with the length of this Vector2 and the direction orthogonal to this Vector2.
   */
  ROS_FORCE_INLINE Vector2 orthogonal() const
  {
    return Vector2(*this).setOrthogonal();
  }
  
  /*!
   * Return this Vector2 with the direction orthogonal to its original direction.
   */
  ROS_FORCE_INLINE Vector2 setOrthogonal()
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_mul_ps(_mm_shuffle_ps(m128_, m128_, _MM_SHUFFLE(3,2,0,1)), _mm_setr_ps(-1.0f, 1.0f, 0.0f, 0.0f));
#else
    const float x = x_;
    
    x_ = -y_;
    y_ = x;    
#endif
    return *this;
  }
  
  /*!
   * Return true if this Vector2 is equal to a given Vector2.
   */
  ROS_FORCE_INLINE bool operator==(const Vector2& vector) const
  {
#ifdef HAS_SSE_EXTENSIONS
    return ((_mm_movemask_ps(_mm_cmpeq_ps(m128_, vector.m128_)) & 3) == 3) != 0;
#else
    return x_ == vector.x_ && y_ == vector.y_;
#endif
  }

  /*!
   * Return true if this Vector2 is not equal to a given Vector2.
   */
  ROS_FORCE_INLINE bool operator!=(const Vector2& vector) const
  {
    return !(*this == vector);
  }

  /*!
   * Return the negative of this Vector2.
   */
  ROS_FORCE_INLINE Vector2 operator-() const
  {
#ifdef HAS_SSE_EXTENSIONS
    return Vector2(_mm_sub_ps(_mm_setzero_ps(), m128_));
#else
    return Vector2(-x_, -y_);
#endif
  }

  /*!
   * Return the compound product of this Vector2 and a given scalar value.
   */
  ROS_FORCE_INLINE Vector2& operator*=(float scalar)
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_mul_ps(m128_, _mm_set1_ps(scalar));
#else
    x_ *= scalar;
    y_ *= scalar;
#endif
    return *this;
  }

  /*!
   * Return the compound inverse product of this Vector2 and a given scalar
   * value.
   */
  ROS_FORCE_INLINE Vector2& operator/=(float scalar)
  {
    ROS_ASSERT(scalar != 0.0f);
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_div_ps(m128_, _mm_set1_ps(scalar));
    
    return *this;
#else
    return *this *= 1.0f / scalar;
#endif
  }

  /*!
   * Return the compound sum of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE Vector2& operator+=(const Vector2& vector)
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_add_ps(m128_, vector.m128_);
#else
    x_ += vector.x_;
    y_ += vector.y_;
#endif
    return *this;
  }

  /*!
   * Return the compound difference of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE Vector2& operator-=(const Vector2& vector)
  {
#ifdef HAS_SSE_EXTENSIONS
    m128_ = _mm_sub_ps(m128_, vector.m128_);
#else
    x_ -= vector.x_;
    y_ -= vector.y_;
#endif
    return *this;
  }

  /*!
   * Return the product of this Vector2 and a given scalar value.
   */
  ROS_FORCE_INLINE Vector2 operator*(float scalar) const
  {
    return Vector2(*this) *= scalar;
  }

  /*!
   * Return the inverse product of this Vector2 and a given scalar value.
   */
  ROS_FORCE_INLINE Vector2 operator/(float scalar) const
  {
    return Vector2(*this) /= scalar;
  }

  /*!
   * Return the sum of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE Vector2 operator+(const Vector2& vector) const
  {
    return Vector2(*this) += vector;
  }

  /*!
   * Return the difference of this Vector2 and a given Vector2.
   */
  ROS_FORCE_INLINE Vector2 operator-(const Vector2& vector) const
  {
    return Vector2(*this) -= vector;
  }

#ifdef HAS_SSE_EXTENSIONS
  __m128 m128_;
#else
  float x_; // x-coordinate.
  float y_; // y-coordinate.
#endif
};

/*!
 * Return a Vector2 with each coordinate equal to the absolute value of the 
 * corresponding coordinate of the given Vector2.
 */
ROS_FORCE_INLINE Vector2 Absolute(const Vector2& vector)
{
  return vector.absolute();
}

/*!
 * \relates Vector2
 *
 * Return the angle in radians between two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Angle(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.angle(vector2);
}

/*!
 * \relates Vector2
 *
 * Return the cross product of two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Cross(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.cross(vector2);
}

/*!
 * \relates Vector2
 *
 * Return the Euclidean distance squared between two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Distance2(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.distance2(vector2);
}

/*!
 * \relates Vector2
 *
 * Return the Euclidean distance between two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Distance(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.distance(vector2);
}

/*!
 * \relates Vector2
 *
 * Return the dot product of two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Dot(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.dot(vector2);
}

/*!
 * \relates Vector2
 *
 * Return the Euclidean length squared of the given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Length2(const Vector2& vector)
{
  return vector.length2();
}

/*!
 * \relates Vector2
 *
 * Return the Euclidean length of the given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Length(const Vector2& vector)
{
  return vector.length();
}

/*!
 * Return a Vector2 with each coordinate equal to the maximum value of the 
 * corresponding coordinate of the two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 Max(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.max(vector2);
}

/*!
 * Return a Vector2 with each coordinate equal to the minimum value of the 
 * corresponding coordinate of the two given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 Min(const Vector2& vector1, const Vector2& vector2)
{
  return vector1.min(vector2);
}
  
/*!
 * \relates Vector2
 *
 * Return a normalized Vector2 with the direction of the given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 Normalized(const Vector2& vector)
{
  return vector.normalized();
}

/*!
 * \relates Vector2
 *
 * Return the orientation in radians the given Vector2 and the positive x-axis.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL float Orientation(const Vector2& vector)
{
  return vector.orientation();
}

/*!
 * \relates Vector2
 *
 * Return a Vector2 orthogonal to the given Vector2.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 Orthogonal(const Vector2& vector)
{
  return vector.orthogonal();
}

/*!
 * Return a Vector2 with each coordinate equal to the value of the corresponding
 * coordinate of the second given Vector2 if the corresponding coordinate of the
 * first given Vector2 is greater than or equal to zero or otherwise equal to
 * the value of the corresponding coordinate of the third Vector2 if the 
 * corresponding coordinate of the first given Vector2 is less than zero.
 */
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 Select(const Vector2& compare_to_zero, const Vector2& greater_than, 
                                                 const Vector2& less_than)
{
#ifdef HAS_SSE_EXTENSIONS
  __m128 m128 = _mm_cmpge_ps(compare_to_zero.m128_, _mm_setzero_ps());
  
  return _mm_or_ps(_mm_and_ps(m128, greater_than.m128_), _mm_andnot_ps(m128, less_than.m128_));
  
#else
  //return Vector2(compare_to_zero.getX() >= 0.0f ? greater_than.getX() : less_than.getX(),
                 //compare_to_zero.getY() >= 0.0f ? greater_than.getY() : less_than.getY());
  return Vector2(FSel(compare_to_zero.getX(), greater_than.getX(), less_than.getX()),
                 FSel(compare_to_zero.getY(), greater_than.getY(), less_than.getY()));
#endif
}
  
/*!
* \relates Vector2
*
* Return the product of a given scalar value and a given Vector2.
*/
ROS_FORCE_INLINE ROS_HELPER_LOCAL Vector2 operator*(float scalar, const Vector2& vector)
{
  return vector * scalar;
}

/*!
* \relates Vector2
*
* Insert a given Vector2 into a given output stream
*/
ROS_FORCE_INLINE ROS_HELPER_LOCAL std::ostream& operator<<(std::ostream& output_stream, const Vector2& vector)
{
  output_stream << "(" << vector.getX() << ", " << vector.getY() << ")";

	return output_stream;
}
  
#ifdef HAS_SSE_EXTENSIONS  
// Return an __m128 containing the dot product of the two lowest floating point
// values in two given __m128.
ROS_FORCE_INLINE ROS_HELPER_LOCAL __m128 DotM128(const __m128 m128a, const __m128 m128b)
{
  __m128 m128 = _mm_mul_ps(m128a, m128b);
  
  m128 = _mm_add_ps(_mm_shuffle_ps(m128, m128, _MM_SHUFFLE(0,0,0,0)), _mm_shuffle_ps(m128, m128, _MM_SHUFFLE(1,1,1,1)));
  
  return m128;
}

// Store the lowest floating point value from the given __m128.
ROS_FORCE_INLINE ROS_HELPER_LOCAL float StoreM128(const __m128 m128)
{
  float scalar;
  
  _mm_store_ss(&scalar, m128);
  
  return scalar;
}
#endif
}

#endif // NAV_RECIP_COMMON_VECTOR2_H_
